# vim:ts=4:sw=4:noet:
"""
Simulator and VM-Sim Interface
"""

import socket
import SocketServer
import logging
import math
import subprocess
from Queue import Queue
from c.base.support import CoreException
from c.base.platform import Platform

class ThreadedTCPRequestHandler(SocketServer.BaseRequestHandler):
	def handle(self):
		logging.info("[sm] Sim client connect")
		try:
			while 1:
				data = self.request.recv(1024).strip().split()
				response = 0
				if len(data)>0:
					cmd = "cmd_"+data[0]
					args = data[1:]
					#print "got cmd", cmd, args
					if hasattr(self.server.sim, cmd):
						response = getattr(self.server.sim, cmd)(args)
						if not response:
							response = 0
					else:
						logging.warn("Unhandled sim request '%s'" % data)
						response = -1
				self.request.send("%d" % response)
		except Exception, e:
			logging.info("[sm] Sim client disconnect: %s" % e)
			self.server.sim.reset()
			return

class ThreadedTCPServer(SocketServer.ThreadingMixIn, SocketServer.TCPServer):
	def setSim(self, sim):
		self.sim = sim

class Simulator:
	def __init__(self, mc):
		self.mc = mc
		host = "localhost"
		port = 2345
		self.cvm_killed = False
		self.server = None
		for i in range(5):
			try:
				self.server = ThreadedTCPServer((host, port), ThreadedTCPRequestHandler)
				break
			except socket.error, e:
				logging.warning("couldn't start sim on %d, trying new port" % port)
				port += 1
		if not self.server:
			raise CoreException("Couldn't start simulator server: %s" % e)
		self.simport = port
		self.server.setSim(self)
		ip, port = self.server.server_address
		logging.info("[sm] Simulator server on %s:%s" % (ip, port))
		self.printbuf = ""
		self.pbqueue = Queue()
		self.mc.tm.createTask("simserver", self.server.serve_forever, daemon=True)
		self.resetPos = (175, 175)
		self.maxPos = (350, 350)
		self.reset()
		self.mc.tm.createTask("cvm", self.cvm_runner)

	def __del__(self):
		print "sim destroy"
		if not self.cvm_killed and hasattr(self, "cvm_p"):
			print "need to kill cvm"
			self.shutdown()

	def shutdown(self):
		logging.info("[sm] Shutting down")
		self.cvm_p.kill()
		self.cvm_killed = True
		self.server.shutdown()

	def cvm_runner(self):
		logging.info("[sm] cvm started")
		self.cvm_p = subprocess.Popen([Platform.getSimCmd(), "-p", "%d" % self.simport])
		self.cvm_p.wait()
		logging.info("[sm] cvm done")

	def reset(self):
		self.pos = self.oldPos = self.resetPos
		self.theta = self.oldTheta = 0
		self.vel = [0,0]
		self.encoder = [0,0]
		self.encoderTarget = [0,0]
		self.radius = 20.0
		self.wheel_radius = 10.0
		self.penDown = False
		self.penWidth = 1
		self.penColour = (0.,0.,0.)
		self.lines = []
		self.linesDrawn = 0
		self.needsReset = True
		self.pbqueue.put("")
		self.bumpers = [0,0,0]
		self.lineSensor = [0,0,0]
		self.addLinePoint()

	def addLinePoint(self):
		self.lines.append((self.pos, self.penColour, self.penWidth, self.penDown))

	def getDistanceFrac(self, d0, de, dtarg):
		d1 = d0+de
		if ((d1 > d0 and dtarg >= d0 and dtarg <= d1) or
			(d1 < d0 and dtarg <= d0 and dtarg >= d1)):
			return (dtarg-d0)/(d1-d0)
		return 1.0 

	def angleInRange(self, angle, low, high):
		angle = angle%360
		low = low%360
		high = high%360
		if high>=low:
			return (angle>=low) and (angle<=high)
		else:
			return (angle>=low) or (angle<=high)

	def checkBumpersAngle(self, ofs):
		B_ANGLE_MAX  = 60
		B_ANGLE_MIN  = 30
		B_ANGLE_BOTH = 30
		angle = math.degrees(self.theta)%360
		if self.angleInRange(angle, ofs-B_ANGLE_MAX, ofs-B_ANGLE_MIN):
			self.bumpers[1] = 1
		if self.angleInRange(angle, ofs+B_ANGLE_MIN, ofs+B_ANGLE_MAX):
			self.bumpers[0] = 1
		if self.angleInRange(angle, ofs-B_ANGLE_BOTH, ofs+B_ANGLE_BOTH):
			self.bumpers = [1,1]
		
	def checkBumpers(self, collx, colly):
		self.bumpers = [0,0]
		if collx==1:
			self.checkBumpersAngle(270)
		elif collx==-1:
			self.checkBumpersAngle(90)
		if colly==1:
			self.checkBumpersAngle(0)
		elif colly==-1:
			self.checkBumpersAngle(180)

	def update(self, dt):
		if self.vel[0]==0 and self.vel[1]==0:
			return
		# get left and right wheel velocities (revs/s), set ticks/rev
		v_l, v_r = self.vel
		tpr = 1800
		# calculate revolutions
		rev_l = v_l * dt
		rev_r = v_r * dt
		# calculate distance traveled for each wheel
		d_l = rev_l * math.pi*2*self.wheel_radius
		d_r = rev_r * math.pi*2*self.wheel_radius
		# get encoder ticks
		enc_l = rev_l * tpr
		enc_r = rev_r * tpr
		# get fraction of update travelled, based on encoder targets
		frac_l = self.getDistanceFrac(self.encoder[0], enc_l, self.encoderTarget[0])
		frac_r = self.getDistanceFrac(self.encoder[1], enc_r, self.encoderTarget[1])

		d_l *= frac_l
		d_r *= frac_r
		enc_l *= frac_l
		enc_r *= frac_r
		# calculate cartesian motion
		d_th = (d_l-d_r)/(2.0*self.radius)
		d_avg = (d_l+d_r)/(2.0)
		d_x = -d_avg*math.sin(self.theta)
		d_y = d_avg*math.cos(self.theta)
		# update
		self.theta += d_th
		newx = self.pos[0] + d_x
		newy = self.pos[1] + d_y
		rd = self.radius/2 + 3
		collx = colly = 0
		if (newx > self.maxPos[0]-rd): 
			newx = self.maxPos[0]-rd
			collx = 1
		if (newx < rd): 
			newx = rd
			collx = -1
		if (newy > self.maxPos[1]-rd): 
			newy = self.maxPos[1]-rd
			colly = 1
		if (newy < rd): 
			newy = rd
			colly = -1
		self.checkBumpers(collx, colly)
		self.pos = (newx, newy)
		self.encoder[0] += enc_l
		self.encoder[1] += enc_r

		"""
		print "rev:(%0.2f,%0.2f) | d:(%0.2f,%0.2f) | enc:(%0.2f,%0.2f) | pos:(%0.2f,%0.2f)" % (
				rev_l, rev_r, 
				d_l, d_r, 
				self.encoder[0], self.encoder[1], #enc_l, enc_r,
				self.encoderTarget[0], self.encoderTarget[1])
		print frac_l, frac_r
		"""
		# update lines
		dx = self.pos[0] - self.oldPos[0]
		dy = self.pos[1] - self.oldPos[1]
		dth = abs(self.theta - self.oldTheta)
		if (math.sqrt((dx*dx)+(dy*dy))>5 or dth>1):
			if self.penDown:
				self.addLinePoint()
			self.oldTheta = self.theta
			self.oldPos = self.pos
	
	def cmd_beep(self, args):
		logging.warn("BEEP %s" % args[0])

	def cmd_reset(self, args):
		self.reset()

	def cmd_stop(self, args):
		self.vel = [0,0]

	def cmd_printchar(self, args):
		c = chr(int(args[0]))
		if c=="\n":
			self.pbqueue.put(self.printbuf)
			self.printbuf = ""
		else:
			self.printbuf += c

	def cmd_setvel(self, args):
		self.vel[int(args[0])] = float(args[1])/135.0

	def cmd_setposition(self, args):
		self.pos = (float(args[0]), float(args[1]))

	def cmd_setangle(self, args):
		self.theta = float(args[0])

	def cmd_pencolour(self, args):
		r, g, b = [float(x)/7.0 for x in args]
		self.penColour = (r,g,b)

	def cmd_penstate(self, args):
		self.addLinePoint()
		self.penDown = int(args[0])==1
		self.addLinePoint()

	def cmd_penstyle(self, args):
		self.penWidth = int(args[0])

	def cmd_getposition(self, args):
		which = int(args[0])
		if which==0:
			return int(self.pos[0])
		elif which==1:
			return int(self.pos[1])
		elif which==2:
			a = math.degrees(self.theta)%360
			return int(round(a, 0))
		return 0

	def cmd_getencoder(self, args):
		which = int(args[0])
		if which==0:
			return int(self.encoder[0])
		elif which==1:
			return int(self.encoder[1])
		return 0

	def cmd_getdigital(self, args):
		which = int(args[0])
		if which >= 0 and which <= 2:
			return self.bumpers[which]
		elif which >= 3 and which <= 5:
			return int(self.lineSensor[which-3])
		return 0

	def cmd_motioncommand(self, args):
		which, value = map(int, args)
		if which == 255:
			dist = 0.001
			if ((abs(self.encoder[0] - self.encoderTarget[0]) < dist) and
				(abs(self.encoder[1] - self.encoderTarget[1]) < dist)):
				return 0
			else:
				return 1
		else:
			self.encoderTarget[which] = value
			return 0

